# -*- coding: utf-8 -*-
"""
Created on Thu Oct 25 17:58:41 2012

@author: -
"""

import os, sys
import Image as Img
import pygame
import cv2
import cv2.cv as cv
from pygame.locals import *
if not pygame.font: print 'Warning, fonts disabled'
if not pygame.mixer: print 'Warning, sound disabled'
import roslib; roslib.load_manifest('ardrone_joy_control')
import rospy
from sensor_msgs.msg import Image
import roslib.packages
from cv_bridge import CvBridge, CvBridgeError
from ardrone_autonomy.msg import Navdata

class PyManMain:
    """The Main PyMan Class - This class handles the main 
    initialization and creating of the Game."""
    
    def __init__(self, width=640,height=480):
        """Initialize"""
        rospy.init_node('ardrone_pygame_node', anonymous=True)  
        rospy.Subscriber("/ardrone/front/image_raw", Image, self.process_image_ardrone)
        """Initialize PyGame"""
        pygame.init()
        """Set the window Size"""
        self.width = width
        self.height = height
        """Create the Screen"""
        
        self.screen = pygame.display.set_mode((self.width
                                               , self.height))
        
        self.image_path= roslib.packages.get_pkg_dir('ardrone_joy_control') + "/images/"
        #Fixed frame        

        
        self.FixedFrame = pygame.sprite.Sprite()
        self.FixedFrame.image = pygame.image.load(self.image_path+'fijo.png')
        self.FixedFrame.rect = self.FixedFrame.image.get_rect()
        self.FixedFrame.rect.topleft = [100, 0]
        self.screen.blit(self.FixedFrame.image, self.FixedFrame.rect)
        
                #World
        self.World = pygame.sprite.Sprite()
        self.World.image = pygame.image.load(self.image_path+'mundo.png')
        self.World.rect = self.FixedFrame.image.get_rect()
        self.World.rect.topleft = [100, 0]
        self.screen.blit(self.World.image, self.World.rect)

    
    def process_image_ardrone(self,image_ros):
        bridge = CvBridge()
        src = bridge.imgmsg_to_cv(image_ros, desired_encoding="passthrough")
        src_rgb = cv.CreateMat(src.height, src.width, cv.CV_8UC3)
        cv.CvtColor(src, src_rgb, cv.CV_BGR2RGB)
#        self.pi = Img.fromstring("L", cv.GetSize(cv_image), cv_image.tostring())
#        print type(self.pi)
        self.pg_img = pygame.image.frombuffer(src_rgb.tostring(), cv.GetSize(src_rgb), "RGB")
        self.screen.blit(self.pg_img, (0,0)) 
#        sys.exit()
        
        
        
        
        
                                               
    def MainLoop(self):
        """This is the Main Loop of the Game"""
        while 1:
            pygame.display.update()
            for event in pygame.event.get():
                if event.type == pygame.QUIT: 
                    sys.exit()
                
                
if __name__ == "__main__":
    MainWindow = PyManMain()
    MainWindow.MainLoop()